#include "StdAfx.h"
#include "stdlib.h"
#include <math.h>
#include ".\obstacle.h"


static const double lfPi = 3.1415926535897931;

Obstacle::Obstacle(void):m_nNumPts(0),m_arrP2ObstaclePoly(NULL),m_bPositionsValid(false)
{
}

Obstacle::~Obstacle(void)
{
	if(m_arrP2ObstaclePoly)
	{
		delete [] m_arrP2ObstaclePoly;
		m_arrP2ObstaclePoly = NULL;
		m_nNumPts = 0;
	}
}

void Obstacle::SetPolySize(int nPoints)
{
	if(nPoints == m_nNumPts)
	{
		return;
	}
	if(m_arrP2ObstaclePoly)
	{
		delete [] m_arrP2ObstaclePoly;
	}
	m_arrP2ObstaclePoly = new Pos2d[nPoints];
	m_nNumPts = nPoints;
	m_bPositionsValid = false;
}


void Obstacle::SetPolyPositionsRandom(const Pos2d p2CenterMin, const Pos2d p2CenterMax, double lfExpectedRadius, 
									  const double * arrLfBboxExclude1, const double * arrLfBboxExclude2)
{
	Pos2d p2Center;
	bool bAvoidedBboxes = false;
	while(!bAvoidedBboxes)
	{
		p2Center[0] = (rand()/(double(RAND_MAX)))*(p2CenterMax[0]-p2CenterMin[0])+p2CenterMin[0];
		p2Center[1] = (rand()/(double(RAND_MAX)))*(p2CenterMax[1]-p2CenterMin[1])+p2CenterMin[1];

		bAvoidedBboxes = true;
		if(arrLfBboxExclude1)
		{
			if(p2Center[0] + 1.5*lfExpectedRadius > arrLfBboxExclude1[0] &&
				p2Center[1] + 1.5*lfExpectedRadius > arrLfBboxExclude1[1] &&
				p2Center[0] - 1.5*lfExpectedRadius < arrLfBboxExclude1[2] &&
				p2Center[1] - 1.5*lfExpectedRadius < arrLfBboxExclude1[3])
			{
				bAvoidedBboxes = false;
			}
		}
		if(bAvoidedBboxes && arrLfBboxExclude2)
		{
			if(p2Center[0] +1.5*lfExpectedRadius > arrLfBboxExclude2[0] &&
				p2Center[1] + 1.5*lfExpectedRadius > arrLfBboxExclude2[1] &&
				p2Center[0] - 1.5*lfExpectedRadius < arrLfBboxExclude2[2] &&
				p2Center[1] - 1.5*lfExpectedRadius < arrLfBboxExclude2[3])
			{
				bAvoidedBboxes = false;
			}
		}
	} 

	double lfThetaStep = 2.0*lfPi/m_nNumPts;

	double lfThetaOffset = 2.0*lfPi*(rand()/(double(RAND_MAX)));

	for(int iWhichPoint = 0; iWhichPoint < m_nNumPts; ++iWhichPoint)
	{
		double lfRandRad = lfExpectedRadius*(0.5+(rand()/(double(RAND_MAX))));
		m_arrP2ObstaclePoly[iWhichPoint][0] = p2Center[0]+lfRandRad*cos(iWhichPoint*lfThetaStep+lfThetaOffset);
		m_arrP2ObstaclePoly[iWhichPoint][1] = p2Center[1]+lfRandRad*sin(iWhichPoint*lfThetaStep+lfThetaOffset);
	}

	m_bPositionsValid = true;
	CalcBbox();
}

void Obstacle::CalcBbox()
{
	if(m_nNumPts < 1)
	{
		return;
	}
	m_arrLfBbox[2] = m_arrLfBbox[0] = m_arrP2ObstaclePoly[0][0];
	m_arrLfBbox[3] = m_arrLfBbox[1] = m_arrP2ObstaclePoly[0][1];

	for(int i = 1; i < m_nNumPts; ++i)
	{
		for(int nAxis = 0; nAxis < 2; ++nAxis)
		{
			if(m_arrP2ObstaclePoly[i][nAxis] < m_arrLfBbox[nAxis])
			{
				m_arrLfBbox[nAxis] = m_arrP2ObstaclePoly[i][nAxis];
			}

			if(m_arrP2ObstaclePoly[i][nAxis] > m_arrLfBbox[2+nAxis])
			{
				m_arrLfBbox[2+nAxis] = m_arrP2ObstaclePoly[i][nAxis];
			}
		}
	}
}
